

#include "xmodule_config.h"

#if MODULE_CHECK_ENABLED( ACCELERATOR ) && SC7A20_CONFIG_ENABLE


#define SC7A20_SLAVE_ADDR       0x19

// shared variable 
extern em_accel_data_array_t m_acc_data; 
extern uint8_t g_sensor_mode ;
extern uint8_t axis_data_sleep[6];
extern uint8_t axis_data_diff_count;
extern uint8_t axis_data_same_count; 

//////////////////////////////////////////////////////////////////////////////
static bool read_byte(uint8_t reg, uint8_t* data)
{
    return ACCEL_REPEAT_READ( SC7A20_SLAVE_ADDR, reg, data, 1 ) == ACCEL_BUS_SUCCESS;
}
static bool write_byte(uint8_t reg, uint8_t data)
{
   return ACCEL_WRITE( SC7A20_SLAVE_ADDR, reg, &data, 1 ) == ACCEL_BUS_SUCCESS;
}
static bool read_bytes(uint8_t reg, uint8_t* pdata, uint16_t len)
{
    return ACCEL_REPEAT_READ( SC7A20_SLAVE_ADDR, reg, pdata, len ) == ACCEL_BUS_SUCCESS;
}


EM_ACCEL_LOCAL
bool em_accel_mounting(em_accel_desc_t* pdesc)
{
    ACCEL_SET_SLAVE_ADDR( SC7A20_SLAVE_ADDR );
    
    int i ;
    for( i = 0; i < 0x10; i ++ ) 
    {
        if( read_byte( 0x0f, &pdesc->chip_id ) && pdesc->chip_id == 0x11) 
        {
            return true;
        }
        ACCEL_OS_DELAY( 10 );
    }
    
    return false;
}


EM_ACCEL_LOCAL
void em_accel_setup(void)
{        
    write_byte( 0x1E, 0x05 ); // Open operate permission
    write_byte( 0x57, 0x00 ); // disable I2C pull-up internal 
    write_byte( 0x1E, 0x00 ); 
}

EM_ACCEL_LOCAL
void em_accel_read_axis( int16_t* data )
{    
    ACCEL_SET_SLAVE_ADDR( SC7A20_SLAVE_ADDR );
    read_bytes( 0x28, (uint8_t*)data, 6 );
}

EM_ACCEL_LOCAL
const em_accel_data_array_t* em_accel_get_fifo(void)
{   
    #if 1
    ACCEL_SET_SLAVE_ADDR( SC7A20_SLAVE_ADDR );
    int i ;
    uint8_t buf_count;
    if( !read_byte( 0x2f, &buf_count ))
    {
        return NULL;
    }
    buf_count = (buf_count & 0x1F);
    
    m_acc_data.has_remain = false;
    LOG_INFO1("[G-SENSOR]: nb = %d", buf_count );
    if( buf_count > EM_ACCEL_DATA_MAX_NB  )
    {
        buf_count = EM_ACCEL_DATA_MAX_NB   ;
        m_acc_data.has_remain = true;
    }
    
    m_acc_data.nb = buf_count * EM_ACCEL_DATA_BYTE_NB;
    uint8_t* pdata = (uint8_t*)m_acc_data.data;
    while( buf_count > 0)
    {
        for( i = 0; i < EM_ACCEL_DATA_BYTE_NB; i ++ )
        {
            read_byte( 0x28 + i, pdata + i ); // read FIFO buffer 
        }
        pdata += EM_ACCEL_DATA_BYTE_NB;
        buf_count --;        
    }
    write_byte( 0x2E, 0x00 ); // switch to bypass mode 
    write_byte( 0x2E, 0x4A ); // switch to stream mode, WTM = 10
    
    int16_t *paxes = (int16_t*)m_acc_data.data;
    for(  i = 0; i < m_acc_data.nb / 2; i++)
    {
        paxes[i] /= 16;
    }
//    em_accel_data_t* p_accel = &m_acc_data.data[0];
//    LOG_INFO3("[G-SENSOR]: x = %d, y = %d, z = %d", p_accel->x, p_accel->y, p_accel->z );
    LOG_INFO1("[G-SENSOR]: buf_count = %d", m_acc_data.nb );
    
    return &m_acc_data;
    #endif

#if 0 
    int i ;
    int16_t axes[3];
    uint8_t* pdata = (uint8_t*)axes;
    for( i = 0; i < EM_ACCEL_DATA_BYTE_NB; i ++ )
    {
        read_byte( 0x28 + i, &pdata[ i ]); // read FIFO buffer 
    }
    LOG_INFO3("[G-SENSOR]: x=%d,y=%d,z=%d",axes[0],axes[1],axes[2]);
    
    m_acc_data.nb = 0;
    m_acc_data.has_remain = false;
    return &m_acc_data;
#endif 
}

EM_ACCEL_LOCAL
void em_accelerator_sleep_check(void)
{
    ACCEL_SET_SLAVE_ADDR( SC7A20_SLAVE_ADDR );
    
    bool restart = true;
    uint8_t data[6];
//    read_bytes( 0x28, data, 6 );

    if( memcmp( data, axis_data_sleep, 6) != 0)
    {
        axis_data_diff_count ++;
        axis_data_same_count = 0;
        if( axis_data_diff_count > 5)
        {
            restart = false;
        }
    }
    else 
    {
        axis_data_diff_count = 0;
        axis_data_same_count ++;
        if( axis_data_same_count >= 10)
        {
            axis_data_same_count = 0;
//            write_byte( 0x18, 0x00 ); 
//            write_byte( 0x18, 0x82 );
        }
    }
    memcpy( axis_data_sleep, data, 6 );
    if( restart )
    {
        accel_timer_start();
    }
}

static 
void em_accelerator_normal_mode(void)
{
    g_sensor_mode = EM_ACCEL_STATE_NORMAL;
}

static
void em_accelerator_sleep_mode(void)
{
    uint8_t dummy; 
    m_em_acc_is_enabled = false; 
    
    write_byte( 0x20, 0x00 ); // CTRL_REG1, Power-Down    
    write_byte( 0x2E, 0x00 ); // disable FIFO
    write_byte( 0x21, 0x19 ); // CTRL_REG2, HPF cut-off 0.2Hz(ODR=25Hz)
    write_byte( 0x22, 0x40 ); // CTRL_REG3, AOI1 enabled, WTM,OVERRUN disable
    write_byte( 0x23, 0x00 ); // CTRL_REG4, 0x00
    write_byte( 0x24, 0x08 ); // CTRL_REG5, INT latch
    write_byte( 0x25, 0x02 ); // CTRL_REG6, INT active low 
    write_byte( 0x20, 0x37 ); // 25Hz, X/Y/Z
    
    write_byte( 0x33, 0x02 ); // AOI1_DURATION, 1/ODR * 5 
    write_byte( 0x32, 0x0A ); // AOI1_THS, 16mg*9 = 144mg 
    read_byte( 0x26, &dummy); // read REFERENCE     
    write_byte( 0x30, 0x2A ); // AOI1_CTRL_REG, enable All direction 
       
    g_sensor_mode = EM_ACCEL_STATE_SLEEP;

    axis_data_diff_count = 0;
    axis_data_same_count = 0;
    
    m_em_acc_is_enabled = true;  
//    accel_timer_start();
}

static
void em_accelerator_fifo_mode(void)
{
    uint8_t dummy;
    m_em_acc_is_enabled = false;
    
    read_byte( 0x31, &dummy ); 
    LOG_INFO1("[G-SENSOR]: AOI1 src = 0x%X", dummy );    
    read_byte( 0x26, &dummy); // read REFERENCE   
    
    write_byte( 0x30, 0x00 ); // AOI1_CTRL_REG, disable ALL 
    write_byte( 0x2E, 0x00 ); // disable FIFO 
    write_byte( 0x20, 0x00 ); // power-down first 
    write_byte( 0x21, 0x00 ); // disable HPF
    read_byte( 0x26, &dummy ); // 
    write_byte( 0x22, 0x06 ); // WTM, OVERRUN
    write_byte( 0x23, 0x88 ); // 2G, DLPF enable
    write_byte( 0x24, 0x48 ); // FIFO enable, INT latch
    
    write_byte( 0x2E, 0x4A ); // stream mode, WTM = 10
    write_byte( 0x20, 0x47 ); // 50Hz, X/Y/Z 
//    write_byte( 0x23, 0x88 ); // 2G, DLPF enable

    m_em_acc_is_enabled = true; 
    g_sensor_mode = EM_ACCEL_STATE_FIFO;
//    accel_timer_start();
}

static
void em_accelerator_stop_mode(void)
{
    uint8_t dummy;
    
    read_byte( 0x31, &dummy );  // INT_REL
    write_byte( 0x20, 0x00 );   // stop 

    g_sensor_mode = EM_ACCEL_STATE_STOP;
}

EM_ACCEL_LOCAL
void em_accel_set(uint8_t accel_status)
{
    switch( accel_status )
    {
        case EM_ACCEL_STATE_STOP:
        em_accelerator_stop_mode();
        break;

        case EM_ACCEL_STATE_SLEEP:
        em_accelerator_sleep_mode();
        break;
        
        case EM_ACCEL_STATE_FIFO:
        em_accelerator_fifo_mode();
        break;

        case EM_ACCEL_STATE_NORMAL:
        em_accelerator_normal_mode();
        break;

        case EM_ACCEL_STATE_CHECK:
        em_accelerator_sleep_check();  
        break;
    }
}


//EM_ACCEL_LOCAL
//void em_accel_register_callback( em_accel_int_evt_handle_t cb)
//{
//    m_local_cb = cb; 
//}

const em_accel_instance_t sc7a20 = 
{
    .mounting = em_accel_mounting,
    .setup = em_accel_setup,
    .get_axes = em_accel_read_axis,
    .get_fifo = em_accel_get_fifo,
    .sleep_check = em_accelerator_sleep_check,
    .set_status = em_accel_set
};

#endif 
